
#include "Matrix.h"
#include <Math.h>

template<>
float CMatrix<2, 2>::Det()
{
	// computes the determinate of a 3x3 matrix using co-factor
	// expansion

	return (matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0]);
}

//
template<>
float CMatrix<3, 3>::Det()
{
	// computes the determinate of a 3x3 matrix using co-factor
	// expansion

	return (	matrix[0][0] * (matrix[1][1] * matrix[2][2] - matrix[2][1] * matrix[1][2]) - 
				matrix[0][1] * (matrix[1][0] * matrix[2][2] - matrix[2][0] * matrix[1][2]) + 
				matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[2][0] * matrix[1][1])	);
}

template<>
float CMatrix<4, 4>::Det()
{
	// computes the determinate of a 3x3 matrix using co-factor
	// expansion

	return (	matrix[0][0] * (matrix[1][1] * matrix[2][2] - matrix[2][1] * matrix[1][2]) - 
				matrix[0][1] * (matrix[1][0] * matrix[2][2] - matrix[2][0] * matrix[1][2]) + 
				matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[2][0] * matrix[1][1])	);
}

CMatrix<2, 2> operator + (CMatrix<2, 2>& left, CMatrix<2, 2>& rhs)
{
	// this function adds two 2x2 matrices together and stores
	// the result in msum
	CMatrix<2, 2> result;

	result[0][0] = left[0][0] + rhs[0][0];
	result[0][1] = left[0][1] + rhs[0][1];
	result[1][0] = left[1][0] + rhs[1][0];
	result[1][1] = left[1][1] + rhs[1][1];

	return result;
} // end Mat_Add_2X2

template<int row, int col>
CMatrix<row, col>	operator + (CMatrix<row, col>& left, CMatrix<row, col>& rhs)
{
	CMatrix<row, col> result;

	for (int i=0; i<row; i++)
	{
		for (int j=0; j<col; j++)
		{
			// insert resulting row,col element
			result[i][j] = left[i][j] + rhs[i][j];
		} // end for col

	} // end for row

	return result;
}

template<int row, int col>
CMatrix<row, col>	operator * (CMatrix<row, col>& left, CMatrix<row, col>& rhs)
{
	CMatrix<row, col> result;

	return result;
}

//inverse to
template<>
int CMatrix<2, 2>::InverseTo(CMatrix<2, 2>& rhs)
{
	// this function computes the inverse of a 2x2 matrix
	// and stores the result in mi

	// compute determinate
	float det = matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0];

	// if determinate is 0 then inverse doesn't exist
	if (fabs(det) < EPSILON_E5)
		return -1;

	float det_inv = 1.0/det;

	// fill in inverse by formula
	rhs[0][0] =  matrix[1][1] * det_inv;
	rhs[0][1] = -matrix[0][1] * det_inv;
	rhs[1][0] = -matrix[1][0] * det_inv;
	rhs[1][1] =  matrix[0][0] * det_inv;

	// return sucess
	return 0;

}

template<>
int CMatrix<3, 3>::InverseTo(CMatrix<3, 3>& rhs)
{
	// this function computes the inverse of a 3x3

	// first compute the determinate to see if there is 
	// an inverse
	float det = matrix[0][0] * (matrix[1][1] * matrix[2][2] - matrix[2][1] * matrix[1][2]) - 
				matrix[0][1] * (matrix[1][0] * matrix[2][2] - matrix[2][0] * matrix[1][2]) + 
				matrix[0][2] * (matrix[1][0] * matrix[2][1] - matrix[2][0] * matrix[1][1]);

	if (fabs(det) < EPSILON_E5)
		return -1;

	// compute inverse to save divides
	float det_inv = 1.0/det;

	// compute inverse using m-1 = adjoint(m)/det(m)
	matrix[0][0] =  det_inv * (matrix[1][1] * matrix[2][2] - matrix[2][1] * matrix[1][2]);
	matrix[1][0] = -det_inv * (matrix[1][0] * matrix[2][2] - matrix[2][0] * matrix[1][2]);
	matrix[2][0] =  det_inv * (matrix[1][0] * matrix[2][1] - matrix[2][0] * matrix[1][1]);

	matrix[0][1] = -det_inv * (matrix[0][1] * matrix[2][2] - matrix[2][1] * matrix[0][2]);
	matrix[1][1] =  det_inv * (matrix[0][0] * matrix[2][2] - matrix[2][0] * matrix[0][2]);
	matrix[2][1] = -det_inv * (matrix[0][0] * matrix[2][1] - matrix[2][0] * matrix[0][1]);

	matrix[0][2] =  det_inv * (matrix[0][1] * matrix[1][2] - matrix[1][1] * matrix[0][2]);
	matrix[1][2] = -det_inv * (matrix[0][0] * matrix[1][2] - matrix[1][0] * matrix[0][2]);
	matrix[2][2] =  det_inv * (matrix[0][0] * matrix[1][1] - matrix[1][0] * matrix[0][1]);

	// return success
	return 0;
}

template<>
int CMatrix<4, 4>::InverseTo(CMatrix<4, 4>& rhs)
{
	float det =  (	matrix[0][0] * ( matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1] ) -
					matrix[0][1] * ( matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0] ) +
					matrix[0][2] * ( matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0] ) );

	// test determinate to see if it's 0
	if (fabs(det) < EPSILON_E5)
		return -1;

	float det_inv  = 1.0f / det;

	rhs.matrix[0][0] =  det_inv * ( matrix[1][1] * matrix[2][2] - matrix[1][2] * matrix[2][1] );
	rhs.matrix[0][1] = -det_inv * ( matrix[0][1] * matrix[2][2] - matrix[0][2] * matrix[2][1] );
	rhs.matrix[0][2] =  det_inv * ( matrix[0][1] * matrix[1][2] - matrix[0][2] * matrix[1][1] );
	rhs.matrix[0][3] = 0.0f; // always 0

	rhs.matrix[1][0] = -det_inv * ( matrix[1][0] * matrix[2][2] - matrix[1][2] * matrix[2][0] );
	rhs.matrix[1][1] =  det_inv * ( matrix[0][0] * matrix[2][2] - matrix[0][2] * matrix[2][0] );
	rhs.matrix[1][2] = -det_inv * ( matrix[0][0] * matrix[1][2] - matrix[0][2] * matrix[1][0] );
	rhs.matrix[1][3] = 0.0f; // always 0

	rhs.matrix[2][0] =  det_inv * ( matrix[1][0] * matrix[2][1] - matrix[1][1] * matrix[2][0] );
	rhs.matrix[2][1] = -det_inv * ( matrix[0][0] * matrix[2][1] - matrix[0][1] * matrix[2][0] );
	rhs.matrix[2][2] =  det_inv * ( matrix[0][0] * matrix[1][1] - matrix[0][1] * matrix[1][0] );
	rhs.matrix[2][3] = 0.0f; // always 0

	rhs.matrix[3][0] = -( matrix[3][0] * rhs.matrix[0][0] + matrix[3][1] * rhs.matrix[1][0] + matrix[3][2] * rhs.matrix[2][0] );
	rhs.matrix[3][1] = -( matrix[3][0] * rhs.matrix[0][1] + matrix[3][1] * rhs.matrix[1][1] + matrix[3][2] * rhs.matrix[2][1] );
	rhs.matrix[3][2] = -( matrix[3][0] * rhs.matrix[0][2] + matrix[3][1] * rhs.matrix[1][2] + matrix[3][2] * rhs.matrix[2][2] );
	rhs.matrix[3][3] = 1.0f; // always 0

	// return success
	return 0;
}

int Solve_2X2_System(CMatrix22& A, CMatrix21& x, CMatrix21& B)
{
	// solves the system AX=B and computes X=A(-1)*B
	// by using cramers rule and determinates

	// step 1: compute determinate of A
	float det_A = A.Det();

	// test if det(a) is zero, if so then there is no solution
	if (fabs(det_A) < EPSILON_E5)
		return -1;

	// step 2: create x,y numerator matrices by taking A and
	// replacing each column of it with B(transpose) and solve

	CMatrix<2, 2> work_mat; // working matrix

	// solve for x /////////////////

	// copy A into working matrix
	work_mat = A;

	// swap out column 0 (x column)
	work_mat.SetColumn(0, B);

	// compute determinate of A with B swapped into x column
	float det_ABx = work_mat.Det();

	// now solve for X00
	x[0][0] = det_ABx/det_A;

	// solve for y /////////////////

	// copy A into working matrix
	work_mat = A;

	// swap out column 1 (y column)
	work_mat.SetColumn(1, B);

	// compute determinate of A with B swapped into y column
	float det_ABy = work_mat.Det();

	// now solve for X01
	x[1][0] = det_ABy/det_A;

	// return success
	return 0;

} // end Solve_2X2_System

int Solve_3X3_System(CMatrix<3, 3>& A, CMatrix<3, 1>& x, CMatrix<3, 1>& B)
{
	// solves the system AX=B and computes X=A(-1)*B
	// by using cramers rule and determinates

	// step 1: compute determinate of A
	float det_A = A.Det();

	// test if det(a) is zero, if so then there is no solution
	if (fabs(det_A) < EPSILON_E5)
		return -1;

	// step 2: create x,y,z numerator matrices by taking A and
	// replacing each column of it with B(transpose) and solve
	CMatrix<3, 3> work_mat; // working matrix

	// solve for x /////////////////

	// copy A into working matrix
	work_mat = A;
	
	// swap out column 0 (x column)
	work_mat.SetColumn(0, B);

	// compute determinate of A with B swapped into x column
	float det_ABx = work_mat.Det();

	// now solve for X00
	x[0][0] = det_ABx/det_A;

	// solve for y /////////////////

	// copy A into working matrix
	work_mat = A;

	// swap out column 1 (y column)
	work_mat.SetColumn(1, B);

	// compute determinate of A with B swapped into y column
	float det_ABy =  work_mat.Det();

	// now solve for X01
	x[1][0] = det_ABy/det_A;

	// solve for z /////////////////

	// copy A into working matrix
	work_mat = A;

	// swap out column 2 (z column)
	work_mat.SetColumn(2, B);

	// compute determinate of A with B swapped into z column
	float det_ABz = work_mat.Det();

	// now solve for X02
	x[2][0] = det_ABz/det_A;

	// return success
	return 0;

} // end Solve_3X3_System

template<>
int CMatrix<2, 2>::SetColumn(int cloumn, CMatrix<2, 1>& B)
{
	matrix[0][cloumn] = B[0][0];
	matrix[1][cloumn] = B[1][0];

	return 0;
}

template<>
int CMatrix<3, 3>::SetColumn(int cloumn, CMatrix<3, 1>& B)
{
	matrix[0][cloumn] = B[0][0];
	matrix[1][cloumn] = B[1][0];
	matrix[2][cloumn] = B[2][0];

	return 0;
}

template<>
int CMatrix<4, 4>::SetColumn(int cloumn, CMatrix<4, 1>& B)
{
	matrix[0][cloumn] = B[0][0];
	matrix[1][cloumn] = B[1][0];
	matrix[2][cloumn] = B[2][0];
	matrix[3][cloumn] = B[3][0];

	return 0;
}

template<>
int CMatrix<4, 3>::SetColumn(int cloumn, CMatrix<4, 1>& B)
{
	matrix[0][cloumn] = B[0][0];
	matrix[1][cloumn] = B[1][0];
	matrix[2][cloumn] = B[2][0];
	matrix[3][cloumn] = B[3][0];

	return 0;
}

template<>
CMatrix<2, 2> operator *(CMatrix<2, 2>& left, CMatrix<2, 2>& rhs)
{
	// this function multiplies two 2x2 matrices together and 
	// and stores the result in mprod
	CMatrix<2, 2> result;

	result[0][0] = left.matrix[0][0] * rhs[0][0] + left.matrix[0][1] * rhs[1][0];
	result[0][1] = left.matrix[0][0] * rhs[0][1] + left.matrix[0][1] * rhs[1][1];

	result[1][0] = left.matrix[1][0] * rhs[0][0] + left.matrix[1][1] * rhs[1][0];
	result[1][1] = left.matrix[1][0] * rhs[0][1] + left.matrix[1][1] * rhs[1][1];

	return result;
}
//
template<>
CMatrix<4, 4> operator *(CMatrix<4, 4>& left, CMatrix<4, 4>& rhs)
{
	// this function multiplies two 4x4 matrices together and 
	// and stores the result in mprod
	// note later we will take advantage of the fact that we know
	// that w=1 always, and that the last column of a 4x4 is
	// always 0
	CMatrix<4, 4> result;

	for (int row=0; row<4; row++)
	{
		for (int col=0; col<4; col++)
		{
			// compute dot product from row of ma 
			// and column of mb
			float sum = 0; // used to hold result

			for (int index=0; index<4; index++)
			{
				// add in next product pair
				sum += (left.matrix[row][index] * rhs.matrix[index][col]);
			} // end for index

			// insert resulting row,col element
			result.matrix[row][col] = sum;

		} // end for col

	} // end for row

	return result;
}

CMatrix22 operator *(CMatrix22& left, CMatrix22& rhs)
{
	CMatrix22 result;

	// this function multiplies two 2x2 matrices together and 
	// and stores the result in mprod
	result[0][0] = left[0][0] * rhs[0][0] + left[0][1] * rhs[1][0];
	result[0][1] = left[0][0] * rhs[0][1] + left[0][1] * rhs[1][1];

	result[1][0] = left[1][0] * rhs[0][0] + left[1][1] * rhs[1][0];
	result[1][1] = left[1][0] * rhs[0][1] + left[1][1] * rhs[1][1];

	return result;
}

CMatrix44 operator *(CMatrix44& left, CMatrix44& rhs)
{
	CMatrix44 result;

	// this function multiplies two 4x4 matrices together and 
	// and stores the result in mprod
	// note later we will take advantage of the fact that we know
	// that w=1 always, and that the last column of a 4x4 is
	// always 0

	for (int row=0; row<4; row++)
	{
		for (int col=0; col<4; col++)
		{
			// compute dot product from row of ma 
			// and column of mb
			float sum = 0; // used to hold result

			for (int index=0; index<4; index++)
			{
				// add in next product pair
				sum += (left[row][index] * rhs[index][col]);
			} // end for index

			// insert resulting row,col element
			result[row][col] = sum;

		} // end for col

	} // end for row

	return result;
}

int Mat_Mul_VECTOR3D_3X3(CVector3d&  va, 
						 CMatrix33& mb,
						 CVector3d&  vprod)
{
	// this function multiplies a VECTOR3D against a 
	// 3x3 matrix - ma*mb and stores the result in mprod

	for (int col=0; col < 3; col++)
	{
		// compute dot product from row of ma 
		// and column of mb
		float sum = 0; // used to hold result

		for (int row=0; row<3; row++)
		{
			// add in next product pair
			sum += ( va[row] * mb[row][col] );
		} // end for index

		// insert resulting col element
		vprod.Set(col, sum);

	} // end for col

	return 0;
}

int Mat_Mul_1X4_4X4(CMatrix14& ma, 
					CMatrix44& mb,
					CMatrix14& mprod)
{
	// this function multiplies a 1x4 matrix against a 
	// 4x4 matrix - ma*mb and stores the result
	// no tricks or assumptions here, just a straight multiply

	for (int col=0; col<4; col++)
	{
		// compute dot product from row of ma 
		// and column of mb
		float sum = 0; // used to hold result

		for (int row=0; row<4; row++)
		{
			// add in next product pair
			sum += ( ma[0][row] * mb[row][col] );
		} // end for index

		// insert resulting col element
		mprod.Set(0, col, sum);

	} // end for col

	return 0;

} // end Mat_Mul_1X4_4X4
//
//////////////////////////////////////////////////////////////////////
//
int Mat_Mul_VECTOR3D_4X4(CVector3d& va, 
						 CMatrix44& mb,
						 CVector3d&  vprod)
{
	// this function multiplies a VECTOR3D against a 
	// 4x4 matrix - ma*mb and stores the result in mprod
	// the function assumes that the vector refers to a 
	// 4D homogenous vector, thus the function assumes that
	// w=1 to carry out the multiply, also the function
	// does not carry out the last column multiply since
	// we are assuming w=1, there is no point

	for (int col=0; col < 3; col++)
	{
		// compute dot product from row of ma 
		// and column of mb
		float sum = 0; // used to hold result
		int row=0;
		for (row=0; row<3; row++)
		{
			// add in next product pair
			sum += ( va[row] * mb[row][col] );
		} // end for index

		// add in last element in column or w*mb[3][col]
		sum += mb[row][col];    

		// insert resulting col element
		vprod.Set(col, sum);

	} // end for col

	return 0;

} // end Mat_Mul_VECTOR3D_4X4


int Mat_Mul_VECTOR3D_4X3(CVector3d& va, 
						 CMatrix43& mb,
						 CVector3d& vprod)
{
	// this function multiplies a VECTOR3D against a 
	// 4x3 matrix - ma*mb and stores the result in mprod
	// the function assumes that the vector refers to a 
	// 4D homogenous vector, thus the function assumes that
	// w=1 to carry out the multiply, also the function
	// does not carry out the last column multiply since
	// we are assuming w=1, there is no point

	for (int col=0; col < 3; col++)
	{
		// compute dot product from row of ma 
		// and column of mb
		float sum = 0; // used to hold result
		int row=0;
		for (row=0; row<3; row++)
		{
			// add in next product pair
			sum += ( va[row] * mb[row][col] );
		} // end for index

		// add in last element in column or w*mb[3][col]
		sum += mb[row][col];      

		// insert resulting col element
		vprod.Set(col, sum);

	} // end for col

	return 0;

} // end Mat_Mul_VECTOR3D_4X3
//
//////////////////////////////////////////////////////////////////////
//
int Mat_Mul_VECTOR4D_4X4(CVector4d& va, 
						 CMatrix44& mb,
						 CVector4d& vprod)
{
	// this function multiplies a VECTOR4D against a 
	// 4x4 matrix - ma*mb and stores the result in mprod
	// the function makes no assumptions

	for (int col=0; col < 4; col++)
	{
		// compute dot product from row of ma 
		// and column of mb
		float sum = 0; // used to hold result

		for (int row=0; row<4; row++)
		{
			// add in next product pair
			sum += ( va[row] * mb[row][col] );
		} // end for index

		// insert resulting col element
		vprod.Set(col, sum);

	} // end for col

	return 0;

} // end Mat_Mul_VECTOR4D_4X4
//
//////////////////////////////////////////////////////////////////////
//
int Mat_Mul_VECTOR4D_4X3(CVector4d& va, 
						 CMatrix44& mb,
						 CVector4d& vprod)
{
	// this function multiplies a VECTOR4D against a 
	// 4x3 matrix - ma*mb and stores the result in mprod
	// the function assumes that the last column of
	// mb is [0 0 0 1]t , thus w just gets replicated
	// from the vector [x y z w]

	for (int col=0; col < 3; col++)
	{
		// compute dot product from row of ma 
		// and column of mb
		float sum = 0; // used to hold result

		for (int row=0; row<4; row++)
		{
			// add in next product pair
			sum += ( va[row] * mb[row][col] );
		} // end for index

		// insert resulting col element
		vprod.Set(col, sum);

	} // end for col

	// copy back w element
	vprod.Set(3, va[3]);

	return 0;

} // end Mat_Mul_VECTOR4D_4X3



template<>
int CMatrix<2, 2>::Identify()
{
	memcpy((void *)(matrix), (void *)&IMAT_2X2, sizeof(matrix));

	return 0;
}

template<>
int CMatrix<3, 3>::Identify()
{
	memcpy((void *)(matrix), (void *)&IMAT_3X3, sizeof(matrix));

	return 0;
}

template<>
int CMatrix<4, 4>::Identify()
{
	memcpy((void *)(matrix), (void *)&IMAT_4X4, sizeof(matrix));

	return 0;
}

template<>
int CMatrix<4, 3>::Identify()
{
	memcpy((void *)(matrix), (void *)&IMAT_4X3, sizeof(matrix));

	return 0;
}

int Mat_Init_2X2(CMatrix22& ma, float m00, float m01, float m10, float m11)
{

	ma.matrix[0][0] = m00; ma.matrix[0][1] = m01;
	ma.matrix[1][0] = m10; ma.matrix[1][1] = m11;

	return 0;
}

int Mat_Init_3X3(CMatrix33& ma, 
                 float m00, float m01, float m02,
                 float m10, float m11, float m12,
                 float m20, float m21, float m22)
{
	ma.matrix[0][0] = m00; ma.matrix[0][1] = m01; ma.matrix[0][2] = m02;
	ma.matrix[1][0] = m10; ma.matrix[1][1] = m11; ma.matrix[1][2] = m12;
	ma.matrix[2][0] = m20; ma.matrix[2][1] = m21; ma.matrix[2][2] = m22;
	
	return 0;
}

int Mat_Init_4X4(CMatrix44& ma, 
                 float m00, float m01, float m02, float m03,
                 float m10, float m11, float m12, float m13,
                 float m20, float m21, float m22, float m23,
                 float m30, float m31, float m32, float m33)

{
// this function fills a 4x4 matrix with the sent data in 
// row major form
	ma.matrix[0][0] = m00; ma.matrix[0][1] = m01; ma.matrix[0][2] = m02; ma.matrix[0][3] = m03;
	ma.matrix[1][0] = m10; ma.matrix[1][1] = m11; ma.matrix[1][2] = m12; ma.matrix[1][3] = m13;
	ma.matrix[2][0] = m20; ma.matrix[2][1] = m21; ma.matrix[2][2] = m22; ma.matrix[2][3] = m23;
	ma.matrix[3][0] = m30; ma.matrix[3][1] = m31; ma.matrix[3][2] = m32; ma.matrix[3][3] = m33;

	return 0;
} // end Mat_Init_4X4